//******************************************************************************
//
//                 Low Cost Vision
//
//******************************************************************************
// Project:        cratedemo
// File:           Crate.hpp
// Description:    Base class for crates.
// Author:         Wouter Langerak & Lukas Vermond
// Notes:
//
// License:        GNU GPL v3
//
// This file is part of cratedemo.
//
// cratedemo is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// cratedemo is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with cratedemo.  If not, see <http://www.gnu.org/licenses/>.
//******************************************************************************

#include <equipletnode/Crate.hpp>

#include <equipletnode/CrateExceptions.hpp>
#include <datatypes/point2.hpp>
#include <datatypes/size3.hpp>
#include <datatypes/Color.hpp>

#include <equipletnode/MiniBall.hpp>

#include <equipletnode/Environment.hpp> 

#include <xml_io/xml.h>

#include <iostream>


	char* Crate::crate_info;
Crate::Crate(
	std::string n,
	datatypes::point2f position,
	float angle,
	bool moving,
	std::vector<CrateContent*>& crateContent) :
		position(position),
		angle(angle),
		moving(moving),
		data(crateContent),
		isScanned(false),
		name(n) {
			//utility_xml::crate_data(crate_info ,name , &crate_width, &crate_height, &crate_depth, 
			//						&grid_width, &grid_height, &distance_to_side, &distance_to_next,
			//		 				&radius_of_ball_container, &bottom_thickness);
			
		crate_width = 45.72;
	    crate_height = 45.72;
	    crate_depth = 16.5;
	    grid_width = 4;
	    grid_height = 4;
	    distance_to_side = 1.11;
	    distance_to_next = 0.5;
	    radius_of_ball_container = 5.25;
	    bottom_thickness = 5.3;
		
			
			size = datatypes::size3f(crate_width, crate_depth, crate_height);
}

Crate::~Crate() {
}

void Crate::put(size_t index, CrateContent* crateContent) {
	if (data.at(index) != NULL)
	{
		throw LocationIsEmptyException();
	}
	data[index] = crateContent;
}

CrateContent* Crate::get(size_t index) const {
	CrateContent* content = data.at(index);
	if(content == NULL) { throw LocationIsFullException(); }
	return content;
}

void Crate::remove(size_t index) {
		if(data.at(index) == NULL) { 
		std::cout << "data is null" << std::endl;throw LocationIsEmptyException(); }
		data[index] = NULL;
}

const std::string& Crate::getName(void) const {
	return name;
}

bool Crate::isEmpty() const {
	for(std::vector<CrateContent*>::iterator it = data.begin();
		it != data.end(); ++ it)
	{
		if (*it != NULL)
		{
			return false;
		}
	}
	return true;
}

bool Crate::isIndexEmpty(size_t index) const {
	if(data.at(index) != NULL) {
		return false;
	}
	
	return true;
}

datatypes::point3f Crate::getContainerLocation(size_t index) const {
	using namespace datatypes;
	std::cout << "crate_width: " << crate_width << ", crate_height: " << crate_height << std::endl;
	point2f location2D;
	location2D.x = (-crate_width/2) + (index%((int)grid_width)) * (2.0 * radius_of_ball_container + distance_to_next) + distance_to_side + radius_of_ball_container/2;
	location2D.y = (-crate_height/2) + (index / grid_height) * (2.0 * radius_of_ball_container + distance_to_next) + distance_to_side + radius_of_ball_container/2;
	location2D = location2D.rotate(-angle);
	location2D += position;
	//std::cout << "TABLE_HEIGHT = " << TABLE_HEIGHT << " bottomThickness = " << bottomThickness << " (TABLE_HEIGHT+bottomThickness) = " << (TABLE_HEIGHT+bottomThickness) << std::endl;
	point3f location3D(location2D.x, location2D.y, (TABLE_HEIGHT+bottom_thickness));
	//std::cout << "getContainerLocation:\nx:\t" << location3D.x << "\ny:\t" << location3D.y << "\nz:\t" << location3D.z << std::endl;
	return location3D;
}

datatypes::point3f Crate::getContentGripLocation(size_t index) const {
	if (data.at(index) == NULL) {
		throw LocationIsEmptyException();
	}

	std::cout << "data.at(index)->getGripPoint().z = " << data.at(index)->getGripPoint().z << std::endl;
	return getContainerLocation(index) + data.at(index)->getGripPoint();
}

void addEmpty(CrateContentMap* crate, const char* name)
{
	std::vector<CrateContent*> vec;
	for(size_t i = 0; i < 4*4; i++)
	{
		vec.push_back(NULL);
	}
	crate->insert(std::pair<std::string, std::vector<CrateContent*> >(name, vec));
}

void add(CrateContentMap* crate, const char* name, datatypes::Color color)
{
	std::vector<CrateContent*> vec;
	for(size_t i = 0; i < 4*4; i++)
	{
		vec.push_back(new MiniBall(color));
	}
	crate->insert(std::pair<std::string, std::vector<CrateContent*> >(name, vec));
}
	
void addCustom(CrateContentMap* crate, const char* name, datatypes::Color color, int x_empty, int y_empty){
	std::vector<CrateContent*> vec;
	for(int y = 0; y < 4; y++){
		for(int x = 0; x < 4; x++)	{
			if(x_empty == x && y_empty == y){
				vec.push_back(NULL);
			} else {
				vec.push_back(new MiniBall(color));
			}
		}
	}
	crate->insert(std::pair<std::string, std::vector<CrateContent*> >(name, vec));
}

